module matrix_utils_mod
    use, intrinsic :: iso_fortran_env, only: real64
    implicit none
    real(real64),parameter::tol=1.0e-12_real64  ! 误差限
    ! ! 1) 声明函数重载接口
    ! interface invert_matrix
    !     module procedure invert_matrix_s   ! 单精度
    !     module procedure invert_matrix_d   ! 双精度
    ! end interface invert_matrix

contains
    !> 输入方阵A, 输出逆矩阵A_inv, 变量ok表示是否成功 ,ok=0表示成功,ok=-1表示失败
    subroutine invert_matrix_s(A, A_inv, ok)
        real, intent(in) :: A(:,:)
        real, intent(out) :: A_inv(:,:)
        integer, intent(out) :: ok
        
        real, allocatable :: aug(:,:)
        integer :: n, i, k, pivot_row
        real :: pivot, factor
        
        n = size(A,1)
        ok = -1  ! 默认失败
        
        ! 检查是否为方阵
        if (size(A,2) /= n) return
        if (size(A_inv,1) /= n .or. size(A_inv,2) /= n) return
        
        ! 创建增广矩阵 [A|I]
        allocate(aug(n, 2*n))
        aug(:,1:n) = A
        aug(:,n+1:2*n) = 0.0
        do i = 1, n
            aug(i,n+i) = 1.0
        end do
        
        ! 高斯-约当消元
        do k = 1, n
            ! 部分主元选择
            pivot_row = k
            do i = k+1, n
                if (abs(aug(i,k)) > abs(aug(pivot_row,k))) pivot_row = i
            end do
            
            if (abs(aug(pivot_row,k)) < 1.0d-12) then
                deallocate(aug)
                return  ! 矩阵奇异
            end if
            
            ! 行交换
            if (pivot_row /= k) then
                aug([k,pivot_row],:) = aug([pivot_row,k],:)
            end if
            
            ! 归一化当前行
            pivot = aug(k,k)
            aug(k,:) = aug(k,:) / pivot
            
            ! 消去其他行
            do i = 1, n
                if (i /= k) then
                    factor = aug(i,k)
                    aug(i,:) = aug(i,:) - factor * aug(k,:)
                end if
            end do
        end do
        
        ! 提取逆矩阵
        A_inv = aug(:,n+1:2*n)
        ok = 0
        deallocate(aug)
    end subroutine invert_matrix_s
    
    ! !> 输入方阵A, 输出逆矩阵A_inv, 变量ok表示是否成功 ,ok=0表示成功,ok=-1表示失败
    ! subroutine invert_matrix_d(A, A_inv, ok)
    !     real(real64), intent(in) :: A(:,:)
    !     real(real64), intent(out) :: A_inv(:,:)
    !     integer, intent(out) :: ok
        
    !     real(real64), allocatable   :: aug(:,:)
    !     integer                     :: n, i, k, pivot_row
    !     real(real64)                :: pivot, factor
        
    !     n = size(A,1)
    !     ok = -1  ! 默认失败
        
    !     ! 检查是否为方阵
    !     if (size(A,2) /= n) return
    !     if (size(A_inv,1) /= n .or. size(A_inv,2) /= n) return
        
    !     ! 创建增广矩阵 [A|I]
    !     allocate(aug(n, 2*n))
    !     aug(:,1:n) = A
    !     aug(:,n+1:2*n) = 0.0_real64
    !     do i = 1, n
    !         aug(i,n+i) = 1.0_real64
    !     end do
        
    !     ! 高斯-约当消元
    !     do k = 1, n
    !         ! 部分主元选择
    !         pivot_row = k
    !         do i = k+1, n
    !             if (abs(aug(i,k)-aug(pivot_row,k)) > tol) pivot_row = i
    !         end do
            
    !         if (abs(aug(pivot_row,k)) < 1.0e-17_real64) then
    !             deallocate(aug)
    !             return  ! 矩阵奇异
    !         end if
            
    !         ! 行交换
    !         if (pivot_row /= k) then
    !             aug([k,pivot_row],:) = aug([pivot_row,k],:)
    !         end if
            
    !         ! 归一化当前行
    !         pivot = aug(k,k)
    !         aug(k,:) = aug(k,:) / pivot
            
    !         ! 消去其他行
    !         do i = 1, n
    !             if (i /= k) then
    !                 factor = aug(i,k)
    !                 aug(i,:) = aug(i,:) - factor * aug(k,:)
    !             end if
    !         end do
    !     end do
        
    !     ! 提取逆矩阵
    !     A_inv = aug(:,n+1:2*n)
    !     ok = 0
    !     deallocate(aug)
    ! end subroutine invert_matrix_d
    
subroutine invert_matrix_d(A, invA, info)
    implicit none
    
    ! 输入参数
    real(real64), intent(in) :: A(:,:)      ! 输入矩阵
    real(real64), intent(out) :: invA(:,:)  ! 输出逆矩阵
    integer, intent(out) :: info       ! 返回信息
    integer:: n           ! 矩阵维度
    
    ! 局部变量
    integer :: lwork, i, j
    integer, allocatable :: ipiv(:)     ! 主元索引数组
    real(real64), allocatable :: work(:)     ! 工作数组
    real(real64),allocatable :: A_temp(:,:)              ! 临时矩阵副本
    
    ! 初始化输出信息
    info = 0
    n=size(A,1)
    allocate(A_temp(n,n))
    ! 检查矩阵维度
    if (n <= 0) then
        info = -1
        return
    endif
    
    ! 复制输入矩阵到临时矩阵（因为LAPACK会覆盖输入矩阵）
    A_temp = A
    
    ! 分配主元索引数组
    allocate(ipiv(n))
    
    ! 第一步：LU分解
    call dgetrf(n, n, A_temp, n, ipiv, info)
    
    if (info /= 0) then
        write(*,*) 'LU failed ,info=', info
        deallocate(ipiv)
        return
    endif
    
    ! 第二步：计算逆矩阵
    ! 首先查询最优工作数组大小
    lwork = -1
    allocate(work(1))
    call dgetri(n, A_temp, n, ipiv, work, lwork, info)
    lwork = nint(work(1))
    deallocate(work)
    
    ! 分配工作数组
    allocate(work(lwork))
    
    ! 计算逆矩阵
    call dgetri(n, A_temp, n, ipiv, work, lwork, info)
    
    if (info /= 0) then
        write(*,*) 'matrix inv failed ,info= ', info
    else
        ! 将结果复制到输出数组
        invA = A_temp
    endif
    
    ! 释放内存
    deallocate(ipiv)
    deallocate(work)
    
end subroutine invert_matrix_d

    ! 计算2*2方阵的行列式
    function det2rank(a) result(det)
        implicit none
        real(real64),intent(in)::a(2,2)
        real(real64)::det
        det=a(1,1)*a(2,2)-a(1,2)*a(2,1)
    end function det2rank

    !>计算一个3x3实数矩阵的行列式,info=0成功; info=1失败
    subroutine det3rank(matrix, det,info)
        implicit none
        ! 声明参数
        real(real64), intent(in) :: matrix(3,3)  ! 输入矩阵
        real(real64), intent(out) :: det         ! 输出行列式
        integer, intent(out) :: info     ! 输出信息
        info=1
        ! 使用三阶行列式的展开公式:
        ! |a b c|
        ! |d e f| = a(ei−fh) − b(di−fg) + c(dh−eg)
        ! |g h i|
        det = matrix(1,1) * (matrix(2,2)*matrix(3,3) - matrix(2,3)*matrix(3,2)) &
            - matrix(1,2) * (matrix(2,1)*matrix(3,3) - matrix(2,3)*matrix(3,1)) &
            + matrix(1,3) * (matrix(2,1)*matrix(3,2) - matrix(2,2)*matrix(3,1))
        info=0
        end subroutine det3rank

    subroutine inverse_diagonal_d(a,inva,ok)
        implicit none
        logical         :: invertible
        integer         :: n  ! 矩阵维度
        integer         :: i, j, ok
        real(real64),intent(in)    :: a(:,:)
        real(real64),intent(out)    :: inva(:,:)
        ok=-1
        invertible = .true.
        n=size(A,1)
        if (size(A,1)/=size(A,2)) then 
            invertible=.false.
            print *, '矩阵不是方阵,不能求逆'
            return 
        end if

        ! 初始化逆矩阵为零矩阵
        invA = 0.0_real64
        
        ! 计算逆矩阵
        do i = 1, n
            if (abs(a(i,i))<1.0e-12_real64 ) then
                invertible = .false.
                exit
            end if
            inva(i,i) = 1.0_real64 / a(i,i)  ! 对角元素取倒数
        end do
        ok=0
    end subroutine inverse_diagonal_d

    !> 去重子程序
    !> 输入: 原数组, 原数组大小
    !> 输出: 去重后的数组, 去重后的大小
    subroutine unique(arr, n, unique_arr, m)
        real(real64), intent(in) :: arr(n)       ! 输入数组
        integer, intent(in) :: n         ! 输入数组大小
        real(real64), intent(out) :: unique_arr(n)  ! 去重后的数组
        integer, intent(out) :: m        ! 去重后的数组大小
        integer :: i, j
        logical :: exists
        
        m = 0  ! 初始化去重数组大小为0
        
        do i = 1, n
            exists = .false.
            ! 检查当前元素是否已存在于去重数组中
            do j = 1, m
                ! 使用微小误差判断浮点数是否相等
                if (abs(arr(i) - unique_arr(j)) < 1e-6_real64) then
                    exists = .true.
                    exit
                end if
            end do
            ! 如果不存在，则添加到去重数组
            if (.not. exists) then
                m = m + 1
                unique_arr(m) = arr(i)
            end if
        end do
    end subroutine unique
    end module matrix_utils_mod